/*
 *  Player - One Hell of a Robot Server
 *  Copyright (C) 2000
 *     Brian Gerkey, Kasper Stoy, Richard Vaughan, & Andrew Howard
 *     Andrew Martignoni III
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program; if not, write to the Free Software
 *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#define HAVE_JPEG 1

#if HAVE_JPEG
  #include <libplayerjpeg/playerjpeg.h>
#endif

#include <assert.h>
#include <stdio.h>
#if !defined (WIN32)
  #include <unistd.h>  /* close(2),fcntl(2),getpid(2),usleep(3),execvp(3),fork(2)*/
#endif
#include <signal.h>  /* for kill(2) */
#include <fcntl.h>   /* for fcntl(2) */
#include <string.h>  /* for strncpy(3),memcpy(3) */
#include <stdlib.h>  /* for atexit(3),atoi(3) */
#include <pthread.h> /* for pthread stuff */
#include <math.h>    /* for rint */
#include <stddef.h>  /* for size_t and NULL */

#include <libplayercore/playercore.h>
#include "vpdetector.h"


/********************************************************************/


// a factory creation function
Driver*
VPDetector_Init( ConfigFile* cf, int section)
{
  return((Driver*)(new VPDetector( cf, section)));
}

// a driver registration function
void
VPDetector_Register(DriverTable* table)
{
  table->AddDriver("VPDetector", VPDetector_Init);
}

VPDetector::VPDetector( ConfigFile* cf, int section)
  : ThreadedDriver(cf, section, true, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
           PLAYER_FIDUCIAL_CODE),
           mCameraDev(NULL),
           mVP(NULL),
           mImg(NULL),
           mWidth(0),
           mHeight(0)
{
  //Canny, Hough and line classification parameters
  vp_params.canny.lowThresh  = cf->ReadInt(section, "canny_lowthresh", 50);
  vp_params.canny.highThresh  = cf->ReadInt(section, "canny_hithresh", 100);
  vp_params.canny.gWindow  = cf->ReadInt(section, "canny_window", 3);
  
  vp_params.hough.method = cf->ReadInt(section, "hough_method", CV_HOUGH_STANDARD);
  vp_params.hough.minLineLength = cf->ReadInt(section, "hough_minlinelen", 50);
  vp_params.hough.rhoRes = cf->ReadFloat(section, "hough_rhores", 1);
  vp_params.hough.angleRes = cf->ReadFloat(section, "hough_angleres", CV_PI/180);
  vp_params.hough.param1 = cf->ReadFloat(section, "hough_param2", 0);
  vp_params.hough.param2 = cf->ReadFloat(section, "hough_param1", 0);
  vp_params.hough.maxLines = cf->ReadInt(section, "hough_maxlines", 50);
  
  vp_params.line.minAngleV = cf->ReadFloat(section, "linef_minanglev", CV_PI/18);
  vp_params.line.maxAngleV = cf->ReadFloat(section, "linef_maxanglev", CV_PI - CV_PI/18);
  vp_params.line.minAngleH = cf->ReadFloat(section, "linef_minangleh", CV_PI/2 + CV_PI/18);
  vp_params.line.maxAngleH = cf->ReadFloat(section, "linef_maxangleh", CV_PI/2 - CV_PI/18);
  vp_params.line.minDiffAngle = cf->ReadFloat(section, "linef_mindifangle", CV_PI/10);
  
  
  
  // Must have an input camera
  if (cf->ReadDeviceAddr(&mCameraAddr, section, "requires",
                         PLAYER_CAMERA_CODE, -1, NULL) != 0)
  {
    PLAYER_ERROR("this driver requires a camera in the .cfg file");
    return;
  }
  
}

VPDetector::~VPDetector()
{
  if (mVP) delete mVP;
  if (mImg) delete mImg;
}

int
VPDetector::MainSetup()
{
  if (mVP)
  {
    PLAYER_ERROR("VanishPoint server already initialized");
    return -1;
  }
  printf("VanishPoint server initializing...");
  fflush(stdout);
  // Subscribe to the camera device
  if (!(this->mCameraDev = deviceTable->GetDevice(this->mCameraAddr)))
  {
    PLAYER_ERROR("unable to locate suitable camera device");
    return(-1);
  }
  if(0 != this->mCameraDev->Subscribe(this->InQueue))
  {
    PLAYER_ERROR("unable to subscribe to camera device");
    return(-1);
  }

  //create vanishing point detector
  mVP = new VanishPoint();
  
  mVP->setCannyParams(vp_params.canny.lowThresh, vp_params.canny.highThresh, vp_params.canny.gWindow);
  
  mVP->setClasiffParams(vp_params.line.minAngleV, vp_params.line.maxAngleV, vp_params.line.minAngleH, vp_params.line.maxAngleH, vp_params.line.minDiffAngle);
  
  mVP->setHoughParams(vp_params.hough.method, vp_params.hough.minLineLength, vp_params.hough.rhoRes, vp_params.hough.angleRes, vp_params.hough.param1, vp_params.hough.param2, vp_params.hough.maxLines);
  
  
  // clean our data
  //memset(&mData,0,sizeof(mData));
  puts("done.\n");
  
  cvNamedWindow( "Final", CV_WINDOW_AUTOSIZE );

  return(0);
}

void 
VPDetector::MainQuit()
{
  // Unsubscribe from the camera
  this->mCameraDev->Unsubscribe(this->InQueue);

  delete mVP; mVP = NULL;
  
  mWidth = 0;
  mHeight = 0;

  puts("VanishPoint server has been shutdown");
}

void
VPDetector::Main()
{
  // The main loop; interact with the device here
  for(;;)
  {
    // wait to receive a new message (blocking)
    Wait();

    // test if we are supposed to cancel
    pthread_testcancel();

    // Process incoming messages, and update outgoing data
    ProcessMessages();
  }
  return;
}

void
VPDetector::ProcessImageData()
{

    assert(mVP);

    //process image
    mVP->process(mImg, false);
    
    //get vanish point
    CvPoint vp = mVP->getVanishPoint();
    
    //if vanish point was detected
    if(vp.x != -1 && vp.y != -1)
    {
      //only one vanishing point is detected
      fdata.fiducials_count = 1;
      fdata.fiducials = new player_fiducial_item_t[1];
      
      
      fdata.fiducials[0].id = (int16_t) -1;
      fdata.fiducials[0].pose.px = vp.x;
      fdata.fiducials[0].pose.py = vp.y;
      
      //printf("vp.x=%d vp.y=%d\n",vp.x,vp.y);
      
      
      this->Publish(device_addr, PLAYER_MSGTYPE_DATA, PLAYER_FIDUCIAL_DATA_SCAN, &this->fdata, sizeof(this->fdata));
    }

}

void
VPDetector::Print()
{

}

int
VPDetector::ProcessMessage(QueuePointer & resp_queue,
                           player_msghdr* hdr,
                           void* data)
{
  
  // Handle new data from the camera
  if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE,
                           this->mCameraAddr))
  {
    // we can't quite do this so easily with camera data
    // because the images are different than the max size
    //assert(hdr->size == sizeof(player_camera_data_t));
    player_camera_data_t* camera_data = reinterpret_cast<player_camera_data_t *>(data);
    uint8_t* ptr;
    uint8_t* ptr1;
    size_t mSize;
    int i;

    assert(camera_data);

    if (camera_data->format != PLAYER_CAMERA_FORMAT_RGB888)
    {
      PLAYER_ERROR("No support for formats other than PLAYER_CAMERA_FORMAT_RGB888");
      return(-1);
    }

#if !HAVE_JPEG
    if (camera_data->compression == PLAYER_CAMERA_COMPRESS_JPEG)
    {
      PLAYER_ERROR("No support for jpeg decompression");
      return(-1);
    }
#endif

    if ((camera_data->width) && (camera_data->height))
    {
      if ((mWidth != camera_data->width) || (mHeight != camera_data->height) || (!mTmp))
      {
        
        mWidth  = camera_data->width;
        mHeight = camera_data->height;
        
        printf("VanishPoint using camera: [w %d h %d]\n", mWidth, mHeight);
        
        if(mWidth != 0 && mHeight != 0)
          //define image size
          mVP->initialize(mWidth, mHeight);
          
        //if (mTmp) delete []mTmp; 
        mTmp = NULL;
        // we need to allocate some memory
        if (!mTmp) mTmp = new uint8_t[mWidth * mHeight * 3];
      }
      ptr = NULL;
      if (camera_data->compression == PLAYER_CAMERA_COMPRESS_JPEG)
      {
        assert(camera_data->bpp == 24);
#if HAVE_JPEG
	jpeg_decompress(reinterpret_cast<unsigned char *>(mTmp),
			mWidth * mHeight * 3,
                        camera_data->image,
                        camera_data->image_count
                       );
#endif
        ptr = mTmp;
      } else switch (camera_data->bpp)
      {
      case 24:
        ptr = camera_data->image;
        break;
      case 32:
        ptr = camera_data->image;
        ptr1 = mTmp;
        mSize = camera_data->width * camera_data->height;
        for (i = 0; i < static_cast<int>(mSize); i++)
        {
          ptr1[0] = ptr[0];
          ptr1[1] = ptr[1];
          ptr1[2] = ptr[2];
          ptr += 4;
          ptr1 += 3;
        }
        ptr = mTmp;
        break;
      default:
        PLAYER_ERROR1("Unsupported depth %u", camera_data->bpp);
        return(-1);
      }
      assert(ptr);
      
      //create new OpenCV image
      mImg = cvCreateImage(cvSize(mWidth,mHeight),IPL_DEPTH_8U,3);

      //copy image ptr to OpenCV image      
      memcpy (mImg->imageData,ptr,mWidth*mHeight*3);

      // find the vanish point
      ProcessImageData();
      
      //after processing, release resources
      cvReleaseImage(&mImg);
      
    }
    return(0);
  }
  // Tell the caller that you don't know how to handle this message
  return(-1);
}

////////////////////////////////////////////////////////////////////////////////
// Extra stuff for building a shared object.

/* need the extern to avoid C++ name-mangling  */
extern "C" {
  int player_driver_init(DriverTable* table)
  {
    puts("VPDetector driver initializing");
    VPDetector_Register(table);
    puts("VPDetector driver done");
    return(0);
  }
}

